#include <lvgl.h>
#include "Arduino_GFX_Library.h"
#include "pin_config.h"
#include "lv_conf.h"
#include <Arduino.h>
#include <Wire.h>
#include "SensorQMI8658.hpp"
#include "HWCDC.h"
#include <WiFi.h>

HWCDC USBSerial;

// 定义传感器数据结构体
struct SensorData {
    bool acc_valid;
    float acc_x, acc_y, acc_z;
    bool gyr_valid;
    float gyr_x, gyr_y, gyr_z;
};

#define EXAMPLE_LVGL_TICK_PERIOD_MS 2

static lv_disp_draw_buf_t draw_buf;
// static lv_color_t draw_buf;
static lv_color_t buf[LCD_WIDTH * LCD_HEIGHT / 10];

SensorQMI8658 qmi;

IMUdata acc;
IMUdata gyr;

lv_obj_t *label_accel;            // Label for Accelerometer data
lv_obj_t *label_gyro;             // Label for Gyroscope data

Arduino_DataBus *bus = new Arduino_ESP32SPI(LCD_DC, LCD_CS, LCD_SCK, LCD_MOSI);

Arduino_GFX *gfx = new Arduino_ST7789(bus, LCD_RST /* RST */,
                                      0 /* rotation */, true /* IPS */, LCD_WIDTH, LCD_HEIGHT, 0, 20, 0, 0);

#if LV_USE_LOG != 0
/* Serial debugging */
void my_print(const char *buf) {
  USBSerial.printf(buf);
  USBSerial.flush();
}
#endif

/* Display flushing */
void my_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p) {
  uint32_t w = (area->x2 - area->x1 + 1);
  uint32_t h = (area->y2 - area->y1 + 1);

#if (LV_COLOR_16_SWAP != 0)
  gfx->draw16bitBeRGBBitmap(area->x1, area->y1, (uint16_t *)&color_p->full, w, h);
#else
  gfx->draw16bitRGBBitmap(area->x1, area->y1, (uint16_t *)&color_p->full, w, h);
#endif

  lv_disp_flush_ready(disp);
}

void example_increase_lvgl_tick(void *arg) {
  /* Tell LVGL how many milliseconds has elapsed */
  lv_tick_inc(EXAMPLE_LVGL_TICK_PERIOD_MS);
}

static uint8_t count = 0;
void example_increase_reboot(void *arg) {
  count++;
  if (count == 30) {
    esp_restart();
  }
}

// --- WiFi 配置 ---
const char* ssid = "unirobot_eu03_back"; // 替换为你的 WiFi SSID
const char* password = "unirobot_eu03_back"; // 替换为你的 WiFi 密码

// --- Socket 配置 ---
const char* server_ip = "192.168.8.111"; // 替换为你的PC端IP
const uint16_t server_port = 9000; // Socket服务端口
WiFiClient socketClient;

void setup() {
  USBSerial.begin(115200); /* prepare for possible serial debug */
  delay(1000); // 等待串口稳定

  // --- 提前初始化 GFX 和 LVGL ---
  gfx->begin();
  pinMode(LCD_BL, OUTPUT);
  pinMode(38, OUTPUT);
  digitalWrite(LCD_BL, HIGH);
  digitalWrite(38, HIGH);
  String LVGL_Arduino = "Hello Arduino! ";
  LVGL_Arduino += String('V') + lv_version_major() + "." + lv_version_minor() + "." + lv_version_patch();

  USBSerial.println(LVGL_Arduino);
  USBSerial.println("I am LVGL_Arduino");

  lv_init();

#if LV_USE_LOG != 0
  lv_log_register_print_cb(my_print); /* register print function for debugging */
#endif

  lv_disp_draw_buf_init(&draw_buf, buf, NULL, LCD_WIDTH * LCD_HEIGHT / 10);

  /*Initialize the display*/
  static lv_disp_drv_t disp_drv;
  lv_disp_drv_init(&disp_drv);
  /*Change the following line to your display resolution*/
  disp_drv.hor_res = LCD_WIDTH;
  disp_drv.ver_res = LCD_HEIGHT;
  disp_drv.flush_cb = my_disp_flush;
  disp_drv.draw_buf = &draw_buf;
  lv_disp_drv_register(&disp_drv);

  /*Initialize the (dummy) input device driver*/
  static lv_indev_drv_t indev_drv;
  lv_indev_drv_init(&indev_drv);
  indev_drv.type = LV_INDEV_TYPE_POINTER;
  lv_indev_drv_register(&indev_drv);

  // 创建用于显示传感器数据的 Label
  label_accel = lv_label_create(lv_scr_act());
  lv_obj_align(label_accel, LV_ALIGN_TOP_LEFT, 10, 10);
  lv_label_set_text(label_accel, "Accel: Initializing...");

  label_gyro = lv_label_create(lv_scr_act());
  lv_obj_align(label_gyro, LV_ALIGN_TOP_LEFT, 10, 40);
  lv_label_set_text(label_gyro, "Gyro: Initializing...");

  const esp_timer_create_args_t lvgl_tick_timer_args = {
    .callback = &example_increase_lvgl_tick,
    .name = "lvgl_tick"
  };

  const esp_timer_create_args_t reboot_timer_args = {
    .callback = &example_increase_reboot,
    .name = "reboot"
  };

  esp_timer_handle_t lvgl_tick_timer = NULL;
  esp_timer_create(&lvgl_tick_timer_args, &lvgl_tick_timer);
  esp_timer_start_periodic(lvgl_tick_timer, EXAMPLE_LVGL_TICK_PERIOD_MS * 1000);

  USBSerial.println("Setup done");

  if (!qmi.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IIC_SDA, IIC_SCL)) {
    USBSerial.println("Sensor Init Failed!");
    while (1) {
      delay(1000);
    }
  }

  /* Get chip id */
  USBSerial.println(qmi.getChipID());

  qmi.configAccelerometer(
    SensorQMI8658::ACC_RANGE_4G,
    SensorQMI8658::ACC_ODR_1000Hz,
    SensorQMI8658::LPF_MODE_0,
    true);

  qmi.configGyroscope(
    SensorQMI8658::GYR_RANGE_64DPS,
    SensorQMI8658::GYR_ODR_896_8Hz,
    SensorQMI8658::LPF_MODE_3,
    true);

  qmi.enableGyroscope();
  qmi.enableAccelerometer();

  qmi.dumpCtrlRegister();

  USBSerial.println("Read data now...");

  // --- WiFi 连接 ---
  USBSerial.printf("Connecting to %s ", ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      USBSerial.print(".");
  }
  USBSerial.println(" connected");
  USBSerial.print("IP address: ");
  USBSerial.println(WiFi.localIP());

  // --- Socket 连接 ---
  USBSerial.printf("Connecting to server %s:%d...", server_ip, server_port);
  if (socketClient.connect(server_ip, server_port)) {
    USBSerial.println(" Socket connected.");
  } else {
    USBSerial.println(" Socket connection failed!");
  }

  USBSerial.println("Setup completed. Starting main loop.");
}

void loop() {
  lv_timer_handler(); /* let the GUI do its work */
  delay(5);

  // 检查Socket连接
  if (!socketClient.connected()) {
    USBSerial.println("[Socket] Disconnected, try reconnect...");
    socketClient.stop();
    if (socketClient.connect(server_ip, server_port)) {
      USBSerial.println("[Socket] Reconnected.");
    } else {
      USBSerial.println("[Socket] Reconnect failed.");
      delay(2000);
      return;
    }
  }

  if (qmi.getDataReady()) {
    SensorData currentData; // 创建一个局部变量来保存当前数据
    currentData.acc_valid = qmi.getAccelerometer(currentData.acc_x, currentData.acc_y, currentData.acc_z);
    currentData.gyr_valid = qmi.getGyroscope(currentData.gyr_x, currentData.gyr_y, currentData.gyr_z);

    if (currentData.acc_valid) {
       char buf_accel[100];
       snprintf(buf_accel, sizeof(buf_accel), "Accel: X:%.2f Y:%.2f Z:%.2f", currentData.acc_x, currentData.acc_y, currentData.acc_z);
       lv_label_set_text(label_accel, buf_accel);
    }
    if (currentData.gyr_valid) {
       char buf_gyro[100];
       snprintf(buf_gyro, sizeof(buf_gyro), "Gyro:  X:%.2f Y:%.2f Z:%.2f", currentData.gyr_x, currentData.gyr_y, currentData.gyr_z);
       lv_label_set_text(label_gyro, buf_gyro);
    }

    // --- 通过Socket发送数据 ---
    if (currentData.acc_valid || currentData.gyr_valid) {
        if (WiFi.status() == WL_CONNECTED && socketClient.connected()) {
            char data_json[200];
            String jsonStr = "";
            if (currentData.acc_valid) {
                jsonStr += "\"accel\":{\"x\":" + String(currentData.acc_x, 2) + ",\"y\":" + String(currentData.acc_y, 2) + ",\"z\":" + String(currentData.acc_z, 2) + "}";
            }
            if (currentData.gyr_valid) {
                if (jsonStr != "") jsonStr += ",";
                jsonStr += "\"gyro\":{\"x\":" + String(currentData.gyr_x, 2) + ",\"y\":" + String(currentData.gyr_y, 2) + ",\"z\":" + String(currentData.gyr_z, 2) + "}";
            }
            if (jsonStr != "") {
                jsonStr = "{" + jsonStr + "}\n"; // 以换行结尾，便于Python端分包
                jsonStr.toCharArray(data_json, 200);
                socketClient.print(data_json);
                USBSerial.print("[Socket] Sent data: ");
                USBSerial.println(jsonStr);
            }
        } else {
            USBSerial.println("[Socket] Not connected. Data not sent.");
        }
    }
  }
  delay(50);  // 增加延迟时间以降低数据发送频率
}
